#include "driver_superc.h"
#include "driver_gimbal.h"
#include "driver_chassis.h"

enum SuperCapState super_cap_state = READY; //电容状态

Super_Cap_t superCap = {
        .scSet.enable = 1
};

//float chassis_power_Judge;
extern receive_judge_t judge_rece_mesg;
extern ChassisSpeedMessegePort RawChassisSpeed;

/*
 * 超级电容发回数据解包
 */
void SC_recv_message(uint8_t *data)
{
    float V_SC;
#if USE_NEW_PROTOCOL == 0
    superCap.scFeedback.V_IN = (int16_t)(data[0] | (data[1] << 8)) / 1000.f;         //mV -> V
    superCap.scFeedback.I_IN_SC = (int16_t)(data[2] | (data[3] << 8)) / 1000.f;      //mA -> A
    V_SC = (int16_t)(data[4] | (data[5] << 8)) / 1000.f;                             //mV -> V
    superCap.scFeedback.V_SC = superCap.scFeedback.V_SC * 0.9f + V_SC * 0.1f;        //低通
    superCap.scFeedback.I_CHASSIS = (int16_t)(data[6] | (data[7] << 8)) / 1000.f;    //mA -> A
#elif USE_NEW_PROTOCOL == 1
    superCap.power_fb.p_sc = (int16_t)(data[0] | (data[1] << 8)) / 50.f;           //0.02W/LSB
    superCap.power_fb.p_wheel = (int16_t)(data[2] | (data[3] << 8)) / 50.f;        //0.02W/LSB
    V_SC = (int16_t)(data[4] | (data[5] << 8)) / 1000.f;                           //mV -> V
    superCap.scFeedback.V_SC = superCap.scFeedback.V_SC * 0.9f + V_SC * 0.1f;      //低通
    super_cap_state = data[6];
#endif
}

void SC_send_message(Super_Cap_t *super_cap)
{
    uint8_t send_buf[8];

    // 电容总开关 1开 0关
    send_buf[0] = super_cap->scSet.enable;
    // 限幅
    super_cap->scSet.set_power_charge = super_cap->scSet.set_power_charge > POWER_CHARGE_MAX ? POWER_CHARGE_MAX : super_cap->scSet.set_power_charge;
    super_cap->scSet.set_power_charge = super_cap->scSet.set_power_charge < -POWER_DISCHARGE_MAX ? -POWER_DISCHARGE_MAX : super_cap->scSet.set_power_charge;
    // 电容功率单位换算 W -> mW -> 0.1mW
    int32_t power_charge = (int32_t)(super_cap->scSet.set_power_charge * 1000 * 10);   // 0.1 mW/LSB//为什么要这样换单位

    send_buf[1] = (uint8_t)(power_charge & 0xff);
    send_buf[2] = (uint8_t)((power_charge >> 8) & 0xff);
    send_buf[3] = (uint8_t)((power_charge >> 16) & 0xff);
    send_buf[4] = (uint8_t)((power_charge >> 24) & 0xff);
    send_buf[5] = (Status.Use_RampC & (1 << 0));

    CAN2_Send_Msg(send_buf, 8, 0x312);
}


extern int32_t LostCounterCountNumber[NUMBERS_OF_COUNT];

PID super_cap_pid;

void SC_PID_Init(void)
{
    super_cap_pid.Kp = SC_KP;
    super_cap_pid.Ki = SC_KI;
    super_cap_pid.Kd = SC_KD;
    super_cap_pid.OutMax =  1;
    super_cap_pid.OutMin = -1;
    super_cap_pid.calc = &PidCalc;
    super_cap_pid.clear = &PidClear;
    super_cap_pid.clear(&super_cap_pid);
}

float Power_pid;
float Power_f;

/*
 * 计算放电功率大小
 */
float SC_power_cacl()
{
    /*
     * 电容功率环
     * 缓冲能量PID + 前馈调节
     *
     *                  抽象电容板
     *      -———————————————————————————————-
     *      |                               |
     *  <————————<————-                     |
     * V_SC |         | I_IN_SC             |
     *      |         ↑                     |
     *  ——>——————>————|                     |
     * V_IN |         ↓ I_CHASSIS           |
     *      |         |                     |
     *      -—————————|—————————————————————-
     *                ↓ Motors
     *
     *  电容如何知道是否应该开始放电？
     *  1. 使用电容PID
     *     当缓冲能量减少时，说明电池放电功率增加，当使用缓冲能量较多超过阈值super_ca_ref时，应开启计算超级电容应放出功率
     *     反之，当缓冲能量剩余低于阈值时，说明裁判系统限制功率未充分利用，考虑给超级电容充电，同时PID避免缓冲能量快速下降导致降速
     *  2. 增加前馈
     *     前馈重要作用是增加响应速度，当底盘功率未到限制功率时，更快给电容充电
     */
    /******  获取裁判系统数据  ******/
    float chassis_power_limit;
    float chassis_power;
    float chassis_power_buffer;

    if((GetErrorState() >> JUDGEMENT_LOST_COUNT) & 1)
    {
        return 0;
    }
    else
    {
        chassis_power_limit = judge_rece_mesg.robot_status_data.chassis_power_limit;
        chassis_power = (float)judge_rece_mesg.power_heat_data.chassis_power;
        chassis_power_buffer = judge_rece_mesg.power_heat_data.chassis_power_buffer;
    }
    float Power_charge=0;

    /****** PID + 前馈  ******/
    if (super_cap_state != READY)
        super_cap_pid.clear(&super_cap_pid);

    Power_f = chassis_power_limit - superCap.power_fb.p_wheel;


    //电容放电时
    if(Power_f<=0)//底盘超功率
    {
        super_cap_pid.Ref = 50 / Buffer_MAX;
        super_cap_pid.Fdb = chassis_power_buffer / Buffer_MAX;
        super_cap_pid.calc(&super_cap_pid);

        //当前缓冲能量chassis_power_buffer小于参考缓冲能量super_cap_ref时(说明底盘功率较大，开始使用缓冲能量了)，此时超级电容开始放电

        Power_pid = -super_cap_pid.Out * Power_delta;
        if(Power_pid>0)
        {
            Power_pid=0;
        }
        Power_charge = Forward_K * Power_f + Power_pid;
        //充放电功率(>0为给电容充电，反之放电)
    }

    //电容充电时
    else
    {
        super_cap_pid.Ref = super_cap_ref / Buffer_MAX;
        super_cap_pid.Fdb = chassis_power_buffer / Buffer_MAX;
        super_cap_pid.calc(&super_cap_pid);
        Power_pid = -super_cap_pid.Out * 40;
        Power_charge = Power_f + Power_pid;
    }

    // 充电限幅
    if(Power_charge > (float)judge_rece_mesg.robot_status_data.chassis_power_limit + 20)
    {
        Power_charge = (float)judge_rece_mesg.robot_status_data.chassis_power_limit + 20;
    }

    // 电容放电功率不能超过底盘轮耗电功率 否则多放出的电会全部木大,反向放给电源管理,白白浪费
    else if (Power_charge < -superCap.power_fb.p_wheel-20)
        Power_charge = -superCap.power_fb.p_wheel-20;

    return Power_charge;
}

/*
 * 超级电容管控
 */
float set_power_charge;
void super_cap_control(void)
{
    /*
     * 新电容控制程序
     * autoSuperFlag仅代表操作手对电容放电与否的控制
     * 操作手不需要也做不到对电容充电与否进行控制 这部分控制（包括防止抢电）应该是电控的任务
     *
     * autoSuperFlag用于追击等需求时开启超级电容，而AutoC是为了飞坡时开启超级电容的标志位
     * AutoC发送给电容控制板，使其短时间内能更快响应放电需求
     *
     * 底盘功率环需要3个输入：缓冲能量 功率上限 实时功率反馈
     * 使用电容放电:       缓冲能量         固定为60J
     *                  底盘功率上限      固定为120w   由电容功率环来保证不会超功率
     *                  实时功率反馈      由电容板测量提供（SC_get_power_wheel()）
     * 不使用电容放电:     缓冲能量         从裁判系统获取
     *                  底盘功率上限      从裁判系统获取
     *                  实时功率反馈      依旧由电容板测量提供（SC_get_power_wheel()）  如果从裁判系统获取会有抢电问题
     */


    set_power_charge = SC_power_cacl();//计算充放电能量

    /******  电容状态切换控制  ******/
    switch (super_cap_state)
    {
        case EMPTY:
        {   // 电容电放完了 不能放电
            set_power_charge = set_power_charge < 0 ? 0 : set_power_charge;
            if(!superCap.scSet.enable && superCap.scSet.set_power_charge > 0) superCap.scSet.enable = 1;
            break;
        }
        case READY:
        {   // 电容就绪 可充可放
            if(!superCap.scSet.enable && superCap.scSet.set_power_charge > 0) superCap.scSet.enable = 1;
            break;
        }
        case FULL:
        {    // 电容电满 不需要继续充电
            set_power_charge = set_power_charge > 0 ? 0 : set_power_charge;
            if(superCap.scSet.enable) superCap.scSet.enable = 0;
            break;
        }
        default:
        {
            set_power_charge = 0;
            break;
        }
    }

    //自动电容开启且电容有电时放电
    if (GimbalReceive.autoSuperFlag && (super_cap_state == READY || super_cap_state == FULL) && !GimbalReceive.rampC)
    {
        if(!superCap.scSet.enable) superCap.scSet.enable = 1;

        if(superCap.scFeedback.V_SC>=19.3f)
        {
            Status.Use_AutoC = 1;
        }
        else
        {
            Status.Use_AutoC = 0;
        }
    }
    else
    {
        Status.Use_AutoC = 0;// 禁止电容放电
    }

    //飞坡电容开启且电容有电时放电
    if(GimbalReceive.rampC && (super_cap_state == READY || super_cap_state == FULL))
    {
        if(!superCap.scSet.enable) superCap.scSet.enable = 1;
        Status.Use_RampC = 1;
    }
    else {
        Status.Use_RampC = 0;
    }

    //未开启使用超级电容，不放电仅充电
    if(!Status.Use_AutoC && !Status.Use_RampC)
    {
        set_power_charge = set_power_charge < 0 ? 0 : set_power_charge;
    }
    else if(Status.Use_RampC)
    {
        //开启飞坡电容时不允许充电
        //避免从充电到放电过程功率变化太大，超级电容跟不上导致裁判系统输出电流超限额，使其自动断电
        set_power_charge = set_power_charge > 0 ? 0 : set_power_charge;
    }

    superCap.scSet.set_power_charge = set_power_charge;

    //将电容充放电指令发送给电容板
    SC_send_message(&superCap);
}

float SC_get_power_wheel(){
    return superCap.power_fb.p_wheel;
}


